#include "pcloud_scans/base_point_cloud_deal.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/conversions.h>
#include <sensor_msgs/Imu.h>
#include <vector>
#include <nav_msgs/Odometry.h>
#include <cmath>
#include <time.h>
#include <tf/transform_datatypes.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h> 
#include <fstream>
#include <iostream>
#include <visualization_msgs/Marker.h>
#include "pcloud_scans/line_extractor.h"
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/filters/voxel_grid.h>
#include <ctime> 
#include "pcloud_scans/fun1.h"
#include "nav_msgs/OccupancyGrid.h"
using namespace std;
ofstream myfile;

//topic
ros::Publisher Lines_pub;
ros::Publisher Scan_pub;
ros::Publisher K_lines_pub;
ros::Publisher Makerpcloud_pub;
ros::Publisher Access1_pub;
ros::Publisher Access2_pub;
ros::Publisher Access3_pub;
ros::Publisher Access4_class;
//ros::Publisher Access5_grid;
//k_means
int Ground_line = 8;//分析地面8根线
int Scan_line = 16;//分析地面8根线
int K_mean = 20, D_mean = 2, N_mean = 0;
//lines
int LINE_MINIMUM_POINTS = 11;
int LINE_MINIMUM_INDEX = 10;
double LINT_AP_DISTANCE = 0.28;//m
double LINE_DIS_THRESHOLD = 0.019;
double LINE_LENGTH = 0.32;
pcl::PointCloud<pcl::PointXYZI>::Ptr Obs_pointcloud (new pcl::PointCloud<pcl::PointXYZI>);

dealScanRegistration::dealScanRegistration(ros::NodeHandle &nh)
{
    ros::Subscriber Sub_point_cloud = nh.subscribe("/velodyne_points", 10, &dealScanRegistration::handleCloudMessage,this);
   // ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu>("/imu/data", 50, handleIMUMessage);
    Makerpcloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/makerpcloud", 10);
    Scan_pub = nh.advertise<sensor_msgs::PointCloud2>("/scan_lines", 10);
    Lines_pub = ros::Publisher(nh.advertise<visualization_msgs::Marker>("/line", 10));
    K_lines_pub = ros::Publisher(nh.advertise<visualization_msgs::Marker>("/K_lines_pub", 10));
    Access1_pub = nh.advertise<sensor_msgs::PointCloud2>("/Access1_pub", 10);
    Access2_pub = nh.advertise<sensor_msgs::PointCloud2>("/Access2_pub", 10);
    Access3_pub = nh.advertise<sensor_msgs::PointCloud2>("/Access3_pub", 10); 
    Access4_class = nh.advertise<visualization_msgs::Marker>("/Access4_class", 10); 
   // Access5_grid = nh.advertise<nav_msgs::OccupancyGrid>("/Access5_grid", 10);
    ROS_INFO("Main Start");
    ROS_INFO("velodyne_16 10hz");
    // fun1();
    ros::spin(); 
}


void dealScanRegistration::process(pcl::PointCloud<pcl::PointXYZI>& laserCloudIn, const Time& scanTime)
{
  //  cout<<endl;
  //  ROS_INFO("Deal Points");
  size_t cloudSize = laserCloudIn.size();
  // determine scan start and end orientations
  float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);  //ori 确定扫描开始和结束方向
  float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
  if (endOri - startOri > 3 * M_PI) 
  {
    endOri -= 2 * M_PI;
  } 
  else if (endOri - startOri < M_PI) 
  {
    endOri += 2 * M_PI;
  }

  bool halfPassed = false;
  _laserCloudScans.resize(_scanMapper.getNumberOfScanRings());//复位
  // clear all scanline points
  std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](auto &&v) {v.clear(); }); 

//#pragma omp for
  // extract valid points from input cloud
  for (int i = 0; i < cloudSize; i++) 
  {
    // skip NaN and INF valued points
    if (!pcl_isfinite(laserCloudIn[i].y) || !pcl_isfinite(laserCloudIn[i].z) ||!pcl_isfinite(laserCloudIn[i].x)) {
      continue;
    }

    // skip zero valued points
    if (laserCloudIn[i].y * laserCloudIn[i].y + laserCloudIn[i].z * laserCloudIn[i].z + laserCloudIn[i].x * laserCloudIn[i].x < 0.0001) {
      continue;
    }

    // calculate vertical point angle and scan ID
    float angle = std::atan(laserCloudIn[i].z / std::sqrt(laserCloudIn[i].y * laserCloudIn[i].y + laserCloudIn[i].x * laserCloudIn[i].x));
    int scanID = _scanMapper.getRingForAngle(angle);
    //ROS_INFO("scanID   %d.", scanID);
    if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){
      continue;
    }

    // calculate horizontal point angle   
    float ori = -std::atan2(laserCloudIn[i].y, laserCloudIn[i].x);
    if (!halfPassed) {
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } else {
      ori += 2 * M_PI;

      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      }
    }

    // calculate relative scan time based on point orientation
    float relTime = 0.1 * (ori - startOri) / (endOri - startOri);
    laserCloudIn[i].intensity =scanID + relTime;   //强度存储   id + 点的相对时间差

    _laserCloudScans[scanID].push_back(laserCloudIn[i]);
  }

  /*for(int i=0;i<16;i++)
  {
    printf("num %d\n",(int)_laserCloudScans[i].size());
  }*/
  //myfile.open("/home/robot/time.txt",ios_base::app);//ios_base::app   ios::out|ios::in|ios::trunc
  //time_t begin_time,end_time;//系统运行时间
  //begin_time=clock();
  get_line();
  Accessible_part1(Ground_line, _laserCloudScans);

  //end_time = clock();
  //cout<<"runtime:   "<<double(end_time-begin_time)/CLOCKS_PER_SEC<<endl;

  /*
  double temp_time = double(end_time-begin_time)/CLOCKS_PER_SEC;
  if(temp_time>0.03)
    temp_time=temp_time-0.005;
  myfile<<temp_time;
  myfile<<",";
  myfile.close();*/

//所有障碍物发布
  Obs_pointcloud->header.frame_id = "velodyne";
  sensor_msgs::PointCloud2 cloud_msg;
  pcl::toROSMsg(*Obs_pointcloud, cloud_msg);
  Access3_pub.publish(cloud_msg);


  /*
    nav_msgs::OccupancyGrid map;
    map.header.frame_id="/velodyne";
    map.header.stamp = ros::Time::now(); 
    map.info.origin.position.x=-15;       // float32
    map.info.origin.position.y=-15;       // float32
    map.info.resolution = 0.1;         // float32
    map.info.width      = 300;           // uint32
    map.info.height     = 300;           // uint32
    map.data.resize(map.info.width * map.info.height);


    for(int i=0; i<Obs_pointcloud->size(); i++)
    {
        double y = Obs_pointcloud->points[i].y;//1m
        double x = Obs_pointcloud->points[i].x;
        int y_cell = y*10;
        int x_cell = x*10;
        if(fabs(y_cell)>=150 || fabs(x_cell)>=150)
            continue;
        map.data[ 150* map.info.width + 150 + y_cell * map.info.width  + x_cell] = -1;

    }
    Access5_grid.publish(map);*/
}



void dealScanRegistration::get_line()
{
 //  ROS_INFO("get_line");
  using namespace visualization_msgs;
  visualization_msgs::Marker l_marker;
  l_marker.action = Marker::ADD;
  l_marker.color.a = 1.0f;
  l_marker.color.r = 1.0f;
  l_marker.color.g = 1.0f;
  l_marker.color.b = 1.0f;
  l_marker.header.frame_id = "/velodyne";
  l_marker.header.stamp = ros::Time::now();
  l_marker.id = 1;
  l_marker.ns = "l_marker";
  l_marker.scale.x = 0.15;
  l_marker.scale.y = 0.15;
  l_marker.scale.z = 0.15;
  l_marker.type = Marker::LINE_LIST;

  std::vector<Point> K_b;
  int Lines_Num[Ground_line][2];//每根线的中线段的数目 最后一个点的id
  std::vector<std::vector<Point_xyset> > All_Lines_Points(2,vector<Point_xyset>(0));//2行，0列
  std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScansOrder;
//滤波点云
  for(int Time=0; Time<Ground_line; Time++)
  {
    //排序
    pcl::PointCloud<pcl::PointXYZI> temp;
    temp =_laserCloudScans[Time];
    /*
    double tmp_1,tmp_2,tmp_3,tmp_4;
   for(int i=0; i<_laserCloudScans[Time].size()-1; i++)
    {
      for(int j=i+1; j<_laserCloudScans[Time].size(); j++)
      {
        if(temp[i].intensity < temp[j].intensity)
        {
          tmp_1 = temp[i].intensity;
          tmp_2 = temp[i].x;
          tmp_3 = temp[i].y;
          tmp_4 = temp[i].z;
          temp[i].intensity = temp[j].intensity;
          temp[i].x = temp[j].x;
          temp[i].y = temp[j].y;
          temp[i].z = temp[j].z;
          temp[j].intensity = tmp_1;
          temp[j].x = tmp_2;
          temp[j].y = tmp_3;
          temp[j].z = tmp_4;
        }
      }
    }*/

    laserCloudScansOrder.push_back(temp);
    //平滑
    for(int i=1; i<_laserCloudScans[Time].size()-1; i++)
    {
        temp[i].x = (temp[i-1].x + temp[i].x + temp[i+1].x)/3;
        temp[i].y = (temp[i-1].y + temp[i].y + temp[i+1].y)/3;
        temp[i].z = (temp[i-1].z + temp[i].z + temp[i+1].z)/3;
    }
  //获取线段
    std::vector<Point> point_cloud;
    for (int i = 0; i < _laserCloudScans[Time].size(); i++) 
    {
      Point p;
      p.x = temp[i].x;
      p.y = temp[i].y;
      p.z = temp[i].z;
      point_cloud.push_back(p);
    }
    std::vector<Line> line;
    lineExtractor(point_cloud, line, LINE_MINIMUM_POINTS, LINE_MINIMUM_INDEX, LINT_AP_DISTANCE, LINE_DIS_THRESHOLD,LINE_LENGTH+Time*0.05);
    reverse(begin(line),end(line));
    Get_key_lines(point_cloud, line);
  //rviz显示线段
  /*
    for (int j=0; j<line.size(); j++) 
    {
      geometry_msgs::Point p;
      p.x = point_cloud.at(line[j].start_index).x;
      p.y = point_cloud.at(line[j].start_index).y;
      l_marker.points.push_back(p);

      p.x = point_cloud.at(line[j].end_index).x;
      p.y = point_cloud.at(line[j].end_index).y;
      l_marker.points.push_back(p);
    }*/
    Lines_Num[Time][0] = line.size();
    Lines_Num[Time][1] = point_cloud.size();
//获取每条线段的Kb
    for (int j=0; j<line.size(); j++) 
    { 
      //记录每个线段的端点pose以及id
      Point_xyset temp;
      temp.x = point_cloud.at(line[j].start_index).x;//First point
      temp.y = point_cloud.at(line[j].start_index).y;
      temp.SET = point_cloud.size() - line[j].start_index;
      //printf("aaaaaaaaaaaaaaaaa %d  %f   %f\n",temp.SET,temp.x,temp.y);
      All_Lines_Points[0].push_back(temp);
      temp.x = point_cloud.at(line[j].end_index).x; //Last point
      temp.y = point_cloud.at(line[j].end_index).y;
      temp.SET = point_cloud.size() - line[j].end_index;
      //printf("aaaaaaaaaaaa %d  %f   %f\n",temp.SET,temp.x,temp.y);
      All_Lines_Points[1].push_back(temp);
//计算线段的K-B
      double d1x = point_cloud.at(line[j].end_index).x - point_cloud.at(line[j].start_index).x;
      double d1y = point_cloud.at(line[j].end_index).y - point_cloud.at(line[j].start_index).y;
      double theta = std::atan2(d1x,d1y)*(180/M_PI);//y坐标向左 和y轴夹角，顺时针0-360
             if(theta<0.0) theta = theta+360;
      double b = 0;
      if(fabs(theta-90)<1e-6 || fabs(theta-270)<1e-6)  
      {
        b = point_cloud.at(line[j].end_index).y;//垂直
        //ROS_INFO("b:  %f    %f", theta, b);/////////////////////////////////////////////////////////
      }
      else if(theta<1e-6 || 360-theta<1e-6 || fabs(theta-180)<1e-6)  
      {
        b = 36.0;//水平
        //ROS_INFO("k:  %f    %f", theta, b);/////////////////////////////////////////////////////////
      }
      else  //x = Ky + B  其中 K = d1x/d1y; B = x1-Ky1;  当x=0时的 截距 b = x1 - ky1
            //b = point_cloud.at(line[j].end_index).y - point_cloud.at(line[j].end_index).x*d1x/d1y;    
      { 
        b = point_cloud.at(line[j].end_index).y - point_cloud.at(line[j].end_index).x*d1y/d1x;    
        if(b>36) b = 36;
        if(b<-36) b = -36;
        //ROS_INFO("k/b:  %f   %f", theta, b);/////////////////////////////////////////////////////////
      }
      Point p;
      p.x = theta;
      p.y = b;
      K_b.push_back(p);
    }
  }


//k_means
  double **K_data = K_array(K_b.size(),D_mean);
  for(int i=K_b.size()-1; i>=0; i--)
  {
    K_data[i][0] = K_b[i].x;
    K_data[i][1] = K_b[i].y;
  }
  N_mean = K_b.size();
  K_mean = 0;

  int Ck = k_means(K_mean, D_mean, N_mean, K_data);
  // ROS_INFO("k_means_over");
  if(Ck>0)
  {
    Kmeans_class Last_class[Ck];//返回结果
    for(int i=0; i<Ck; i++)//分配空间
    {
      Last_class[i].detial.resize(Ground_line,vector<Kmeans_line>(0));
      Last_class[i].Class = i;
    }

    k_select(Ck, K_data, Lines_Num, Ground_line, All_Lines_Points, Last_class);

    lines_accessible_area(Last_class, Ck, Ground_line, laserCloudScansOrder);
  }


 //  ROS_INFO("get_line_over");
//viewer in rviz


  pcl::PointCloud<pcl::PointXYZI>::Ptr temp_filter_ptr (new pcl::PointCloud<pcl::PointXYZI>);
  for(int i=0; i<Ground_line; i++)
  {
      *temp_filter_ptr = *temp_filter_ptr+laserCloudScansOrder[i];
  }
  Scanlinesviewer(temp_filter_ptr);
   Lines_pub.publish(l_marker);


}




//////////////////////////////////////////////////////Scanlinesviewer
//https://blog.csdn.net/xiaoma_bk/article/details/83690960
void Scanlinesviewer(pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
  // construct sorted full resolution cloud   构建排序的全分辨率云
    out->header.frame_id = "/velodyne";
    sensor_msgs::PointCloud2 cloud_msg;

    pcl::toROSMsg(*out, cloud_msg);
    Scan_pub.publish(cloud_msg);
}

////////////////////////////////////////////////////////



int MultiScanMapper::getRingForAngle(const float& angle) {
  return int(((angle * 180 / M_PI) - _lowerBound) * _factor + 0.5);
}


void dealScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
  pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
  ROS_INFO("handleCloudMessage");
  process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp));
}